/**
* Real-time SLAM with webcam/camera input
* Based on ORB-SLAM2 and CubeSLAM
*/

#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include <iomanip>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/videoio.hpp>
#include "Parameters.h"
#include "System.h"

using namespace std;

int main(int argc, char **argv)
{
    if(argc < 3 || argc > 4)
    {
        cerr << endl << "Usage: ./mono_realtime path_to_vocabulary path_to_settings [camera_id]" << endl;
        cerr << "Example: ./mono_realtime ./Vocabulary/ORBvoc.txt Examples/config/Realtime.yaml 0" << endl;
        return 1;
    }

    // Camera ID (default 0 for webcam)
    int camera_id = 0;
    if(argc == 4)
    {
        camera_id = atoi(argv[3]);
    }

    // Open camera first and test it
    cv::VideoCapture cap(camera_id);
    if(!cap.isOpened())
    {
        cerr << "Error: Cannot open camera " << camera_id << endl;
        cerr << "Please check:" << endl;
        cerr << "1. Camera is connected" << endl;
        cerr << "2. Camera is not used by other applications" << endl;
        cerr << "3. User has permission to access camera" << endl;
        return 1;
    }

    // Set camera properties
    cap.set(cv::CAP_PROP_FRAME_WIDTH, 640);
    cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
    cap.set(cv::CAP_PROP_FPS, 30);
    
    // Test camera by reading a frame
    cv::Mat test_frame;
    cap >> test_frame;
    if(test_frame.empty())
    {
        cerr << "Error: Camera opened but cannot read frames" << endl;
        cerr << "Trying alternative camera settings..." << endl;
        
        // Try different backends
        cap.release();
        cap.open(camera_id, cv::CAP_V4L2);
        if(!cap.isOpened())
        {
            cap.open(camera_id, cv::CAP_ANY);
        }
        
        if(!cap.isOpened())
        {
            cerr << "Error: Cannot open camera with any backend" << endl;
            return 1;
        }
        
        // Test again
        cap >> test_frame;
        if(test_frame.empty())
        {
            cerr << "Error: Still cannot read frames from camera" << endl;
            return 1;
        }
    }
    
    cout << "Camera test successful. Frame size: " << test_frame.cols << "x" << test_frame.rows << endl;
    
    // Now create SLAM system after camera is confirmed working
    cout << "Creating SLAM system..." << endl;
    ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::MONOCULAR, true);
    cout << "SLAM system created successfully!" << endl;

    cout << endl << "-------" << endl;
    cout << "Starting real-time SLAM..." << endl;
    cout << "Camera ID: " << camera_id << endl;
    cout << "Press 'q' to quit, 'r' to reset" << endl << endl;

    cv::Mat im;
    int frame_count = 0;
    auto start_time = std::chrono::steady_clock::now();

    while(true)
    {
        // Capture frame
        cap >> im;
        if(im.empty())
        {
            cerr << "Failed to capture frame at frame " << frame_count << endl;
            cerr << "Trying to recover..." << endl;
            
            // Try to recover by reopening camera
            cap.release();
            usleep(100000); // Wait 100ms
            cap.open(camera_id);
            if(!cap.isOpened())
            {
                cerr << "Cannot reopen camera. Exiting..." << endl;
                break;
            }
            cap.set(cv::CAP_PROP_FRAME_WIDTH, 640);
            cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
            cap.set(cv::CAP_PROP_FPS, 30);
            continue;
        }

        // Get timestamp
        auto current_time = std::chrono::steady_clock::now();
        double timestamp = std::chrono::duration_cast<std::chrono::duration<double>>(current_time - start_time).count();

        // Process frame
        SLAM.TrackMonocular(im, timestamp, frame_count, "frame_" + to_string(frame_count));

        // Display frame
        cv::imshow("Real-time SLAM", im);
        
        // Handle keyboard input
        char key = cv::waitKey(1) & 0xFF;
        if(key == 'q' || key == 27) // 'q' or ESC
        {
            cout << "Quitting..." << endl;
            break;
        }
        else if(key == 'r') // 'r' for reset
        {
            cout << "Resetting SLAM..." << endl;
            SLAM.Reset();
        }

        frame_count++;
    }

    // Cleanup
    cap.release();
    cv::destroyAllWindows();
    
    // Stop all threads
    SLAM.Shutdown();

    // Save camera trajectory
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory_realtime.txt");

    cout << "Real-time SLAM finished!" << endl;
    return 0;
}
